package at.jku.pervasive.gui;

import java.awt.BorderLayout;
import java.awt.Color;
import java.awt.Dimension;
import java.awt.GraphicsConfiguration;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collection;
import java.util.LinkedList;
import java.util.List;
import java.util.Map;
import java.util.TreeMap;

import javax.media.j3d.Appearance;
import javax.media.j3d.BoundingSphere;
import javax.media.j3d.BranchGroup;
import javax.media.j3d.Canvas3D;
import javax.media.j3d.ColoringAttributes;
import javax.media.j3d.DirectionalLight;
import javax.media.j3d.GeometryArray;
import javax.media.j3d.Group;
import javax.media.j3d.LineAttributes;
import javax.media.j3d.LineStripArray;
import javax.media.j3d.Material;
import javax.media.j3d.Shape3D;
import javax.media.j3d.Transform3D;
import javax.media.j3d.TransformGroup;
import javax.swing.JPanel;
import javax.vecmath.Color3f;
import javax.vecmath.Point3d;
import javax.vecmath.Tuple3d;
import javax.vecmath.Vector2d;
import javax.vecmath.Vector3d;
import javax.vecmath.Vector3f;

import at.jku.pervasive.Application;
import at.jku.pervasive.Helper;
import at.jku.pervasive.Position;
import at.jku.pervasive.TagAssignment;

import com.sun.j3d.utils.behaviors.vp.OrbitBehavior;
import com.sun.j3d.utils.geometry.Box;
import com.sun.j3d.utils.geometry.Primitive;
import com.sun.j3d.utils.geometry.Sphere;
import com.sun.j3d.utils.universe.SimpleUniverse;
import com.sun.j3d.utils.universe.ViewingPlatform;

public class View3DPanel extends JPanel
{

	double height = 2.8;
	double length = 15.0;
	double width = 10.0;

	double xOffset = 7.0;
	double yOffset = 0.0;
	double zOffset = 0.0;

	private final Point3d origin = new Point3d(0.0, 0.0, 0.0);
	private final Point3d offset = new Point3d(xOffset, yOffset, zOffset);
	private final Point3d centerOfRoom;

	// holds all the lines
	private BranchGroup bodyBranchGroup;

	// holds all tag positions
	private BranchGroup tagsBranchGroup;

	// holds the branch group for each tag position
	private final Map<TagAssignment, BranchGroup> tagPositionBranchGroupMap = new TreeMap<TagAssignment, BranchGroup>();
	private final Map<TagAssignment, BranchGroup> filteredTagPositionBranchGroupMap = new TreeMap<TagAssignment, BranchGroup>();

	// needed for resetting view
	private TransformGroup transformGroup;
	private ViewingPlatform viewingPlatform;

	private Map<TagAssignment, Position> recentTagPositions = new TreeMap<TagAssignment, Position>();
	private Map<TagAssignment, Position> filteredTagPositions = new TreeMap<TagAssignment, Position>();

	// flags which define if the raw and filtered positions shall be shown
	private boolean showRawPositions;
	private boolean showFilteredPositions;
	private boolean showBody;

	public View3DPanel()
	{
		super();

		this.centerOfRoom = new Point3d(offset);
		this.centerOfRoom.add(new Point3d(width / 2.0, length / 2.0, height / 2.0));

		setLayout(new BorderLayout());

		GraphicsConfiguration configuration = SimpleUniverse.getPreferredConfiguration();
		Canvas3D canvas3d = new Canvas3D(configuration);

		add(BorderLayout.CENTER, canvas3d);

		SimpleUniverse universe = new SimpleUniverse(canvas3d);
		BranchGroup objectRoot = createBranchGroup();
		universe.addBranchGraph(objectRoot);

		OrbitBehavior orbit = new OrbitBehavior(canvas3d, OrbitBehavior.REVERSE_ALL | OrbitBehavior.STOP_ZOOM);
		BoundingSphere sphere = new BoundingSphere(origin, 1000.0);
		orbit.setSchedulingBounds(sphere);
		orbit.setZoomFactor(10.0);
		orbit.setTransFactors(5.0, 5.0);
		orbit.setMinRadius(0.5d);
		orbit.setBounds(sphere);
		orbit.setRotationCenter(centerOfRoom);

		viewingPlatform = universe.getViewingPlatform();
		viewingPlatform.setViewPlatformBehavior(orbit);

		resetView();

		setPreferredSize(new Dimension(650, 650));
	}

	public void clearAllPoints()
	{
		this.tagsBranchGroup.removeAllChildren();
	}

	public void removePointForTagId(TagAssignment tagAssignment)
	{
		BranchGroup branchGroupToRemove = this.tagPositionBranchGroupMap.get(tagAssignment);
		if (branchGroupToRemove != null)
			tagsBranchGroup.removeChild(branchGroupToRemove);

		branchGroupToRemove = this.filteredTagPositionBranchGroupMap.get(tagAssignment);
		if (branchGroupToRemove != null)
			tagsBranchGroup.removeChild(branchGroupToRemove);
	}

	public void resetView()
	{
		TransformGroup viewTransformGroup = viewingPlatform.getMultiTransformGroup().getTransformGroup(0);

		Transform3D viewTransform3D = new Transform3D();
		Transform3D xRot = new Transform3D();
		xRot.rotX(Math.PI / 3.0);
		Transform3D zRot = new Transform3D();
		zRot.rotZ(Math.PI / 4.0);
		zRot.mul(xRot);
		viewTransform3D.mul(zRot);
		viewTransform3D.setTranslation(new Vector3d(xOffset + 19.0f, yOffset + -7.0f, zOffset + 14.0f));
		viewTransformGroup.setTransform(viewTransform3D);

		bodyBranchGroup.removeAllChildren();
	}

	public void setShowBody(boolean showBody)
	{
		this.showBody = showBody;

		if (!showBody)
			bodyBranchGroup.removeAllChildren();
		else
		{
		}
	}

	public void setShowFilteredPositions(boolean showFilteredPositions)
	{
		this.showFilteredPositions = showFilteredPositions;

		Collection<BranchGroup> filteredBranchGroups = filteredTagPositionBranchGroupMap.values();
		for (BranchGroup branchGroup : filteredBranchGroups)
		{
			if (!showFilteredPositions)
				tagsBranchGroup.removeChild(branchGroup);
			else
				tagsBranchGroup.addChild(branchGroup);
		}
	}

	public void setShowRawPositions(boolean showRawPositions)
	{
		this.showRawPositions = showRawPositions;

		Collection<BranchGroup> branchGroups = tagPositionBranchGroupMap.values();
		for (BranchGroup branchGroup : branchGroups)
		{
			if (!showRawPositions)
				tagsBranchGroup.removeChild(branchGroup);
			else
				tagsBranchGroup.addChild(branchGroup);
		}
	}

	public void tagAssignmentChanged(TagAssignment tagAssignment, String tagId)
	{
		// empty
	}

	public void updateFilteredTagPosition(TagAssignment tagAssignment, Position filteredPosition)
	{
		filteredTagPositions.put(tagAssignment, filteredPosition);

		// is there already a branch group in the map?
		BranchGroup tagPositionBranchGroup = filteredTagPositionBranchGroupMap.get(tagAssignment);

		if (tagPositionBranchGroup == null)
		{
			// set new point
			tagPositionBranchGroup = new BranchGroup();
			tagPositionBranchGroup.setCapability(BranchGroup.ALLOW_DETACH);
			tagPositionBranchGroup.setCapability(Group.ALLOW_CHILDREN_EXTEND);
			tagPositionBranchGroup.setCapability(Group.ALLOW_CHILDREN_READ);
			tagPositionBranchGroup.setCapability(Group.ALLOW_CHILDREN_WRITE);

			// set branch group for this tag position
			filteredTagPositionBranchGroupMap.put(tagAssignment, tagPositionBranchGroup);

			// get the shape for this tag position
			Primitive shape = getShapeForFilteredTag(tagAssignment);

			Transform3D tmpTrans = new Transform3D();
			Vector3d vector = getVectorForPosition(filteredPosition);
			tmpTrans.setTranslation(vector);
			TransformGroup tmpTransGroup = new TransformGroup();
			tmpTransGroup.setCapability(Group.ALLOW_CHILDREN_READ);
			tmpTransGroup.setCapability(Group.ALLOW_CHILDREN_WRITE);
			tmpTransGroup.setTransform(tmpTrans);
			tmpTransGroup.addChild(shape);
			tmpTransGroup.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
			tagPositionBranchGroup.addChild(tmpTransGroup);

			if (showFilteredPositions)
				tagsBranchGroup.addChild(tagPositionBranchGroup);

			return;
		}

		// get the transform group
		TransformGroup transGroup = (TransformGroup) tagPositionBranchGroup.getChild(0);
		Transform3D transform = new Transform3D();
		transform.setTranslation(getVectorForPosition(filteredPosition));
		transGroup.setTransform(transform);

		if (showBody)
			updateBody();
	}

	private void updateBody()
	{
		// draw the body, if all filtered positions are available
		if (filteredTagPositions.size() != 4)
			return;

		Position leftPos = filteredTagPositions.get(TagAssignment.LEFT);
		Position refPos = calculateReferencePosition();
		Position rightPos = filteredTagPositions.get(TagAssignment.RIGHT);

		Vector3d refPosPoint = getVectorForPosition(refPos);
		refPosPoint.z -= Application.getInstance().getZOffset();
		Point3d leftFoot = new Point3d(-0.05d, 0.0, 0.0);
		Point3d leftHip = new Point3d(-0.05d, 0.0, refPosPoint.z);
		Point3d rightHip = new Point3d(0.05d, 0.0, refPosPoint.z);
		Point3d rightFoot = new Point3d(0.05d, 0.0, 0.0);

		List<Point3d> points = new ArrayList<Point3d>();
		List<Point3d> legs = Arrays.asList(leftFoot, leftHip, rightHip, rightFoot);
		List<Point3d> backbone = Arrays.asList(new Point3d(0.0, 0.0, refPosPoint.z), new Point3d(0.0, 0.0,
				getVectorForPosition(refPos).z));
		points.addAll(legs);
		points.addAll(backbone);

		int[] stripLength = new int[] { legs.size(), backbone.size() };
		LineStripArray bodyArray = new LineStripArray(points.size(), GeometryArray.COORDINATES, stripLength);
		bodyArray.setCoordinates(0, points.toArray(new Point3d[points.size()]));

		List<Point3d> arms = getPointListForPositions(leftPos, refPos, rightPos);
		LineStripArray armArray = new LineStripArray(arms.size(), GeometryArray.COORDINATES, new int[] { arms.size() });
		armArray.setCoordinates(0, arms.toArray(new Point3d[arms.size()]));

		BranchGroup bg = new BranchGroup();
		bg.setCapability(BranchGroup.ALLOW_DETACH);
		bg.setCapability(Group.ALLOW_CHILDREN_EXTEND);

		// calculate angle between back and front
		Position back = filteredTagPositions.get(TagAssignment.BACK);
		Position front = filteredTagPositions.get(TagAssignment.FRONT);

		Vector3d backVec = getVectorForPosition(back);
		Vector3d frontVec = getVectorForPosition(front);
		double angle = caclulateXYAngle(backVec, frontVec);
		angle += Math.PI / 2.0;

		refPosPoint.z = 0.0;
		Vector3d negate = new Vector3d(refPosPoint);
		negate.negate();

		Transform3D rotate = new Transform3D();
		rotate.rotZ(angle);

		Transform3D transformBack = new Transform3D();
		transformBack.setTranslation(refPosPoint);

		transformBack.mul(rotate);

		TransformGroup bodyTransGroup = new TransformGroup(transformBack);
		Shape3D body = new Shape3D(bodyArray);
		Appearance bodyAppearance = getAppearance(Color.LIGHT_GRAY);
		LineAttributes lineAttributes = new LineAttributes(3f, LineAttributes.PATTERN_SOLID, false);
		bodyAppearance.setLineAttributes(lineAttributes);
		body.setAppearance(bodyAppearance);
		bodyTransGroup.addChild(body);

		bg.addChild(bodyTransGroup);

		BranchGroup armBranchGroup = new BranchGroup();
		Shape3D arm = new Shape3D(armArray);
		Appearance armAppearance = getAppearance(Color.RED.brighter());
		LineAttributes armLineAttributes = new LineAttributes(3f, LineAttributes.PATTERN_SOLID, false);
		armAppearance.setLineAttributes(armLineAttributes);
		arm.setAppearance(armAppearance);
		armBranchGroup.addChild(arm);
		bg.addChild(armBranchGroup);

		bodyBranchGroup.removeAllChildren();
		bodyBranchGroup.addChild(bg);
	}

	private double caclulateXYAngle(Tuple3d point1, Tuple3d point2)
	{
		// get angle in XY
		Vector2d point1XY = new Vector2d(point1.x - point2.x, point1.y - point2.y);
		Vector2d point2XY = new Vector2d(1.0, 0.0);

		return point2XY.angle(point1XY);
	}

	public void updateTagPosition(TagAssignment tagAssignment, Position newPosition)
	{
		// add new position as recent tag position
		this.recentTagPositions.put(tagAssignment, newPosition);

		// is there already a branch group in the map?
		BranchGroup tagPositionBranchGroup = tagPositionBranchGroupMap.get(tagAssignment);

		if (tagPositionBranchGroup == null)
		{
			// set new point
			tagPositionBranchGroup = new BranchGroup();
			tagPositionBranchGroup.setCapability(BranchGroup.ALLOW_DETACH);
			tagPositionBranchGroup.setCapability(Group.ALLOW_CHILDREN_EXTEND);
			tagPositionBranchGroup.setCapability(Group.ALLOW_CHILDREN_READ);
			tagPositionBranchGroup.setCapability(Group.ALLOW_CHILDREN_WRITE);

			// set branch group for this tag position
			tagPositionBranchGroupMap.put(tagAssignment, tagPositionBranchGroup);

			// get the shape for this tag position
			Primitive shape = getShapeForTag(tagAssignment);

			Transform3D tmpTrans = new Transform3D();
			Vector3d vector = getVectorForPosition(newPosition);
			tmpTrans.setTranslation(vector);
			TransformGroup tmpTransGroup = new TransformGroup();
			tmpTransGroup.setCapability(Group.ALLOW_CHILDREN_READ);
			tmpTransGroup.setCapability(Group.ALLOW_CHILDREN_WRITE);
			tmpTransGroup.setTransform(tmpTrans);
			tmpTransGroup.addChild(shape);
			tmpTransGroup.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
			tagPositionBranchGroup.addChild(tmpTransGroup);

			if (showRawPositions)
				tagsBranchGroup.addChild(tagPositionBranchGroup);
			return;
		}

		// get the transform group
		TransformGroup transGroup = (TransformGroup) tagPositionBranchGroup.getChild(0);
		Transform3D transform = new Transform3D();
		transform.setTranslation(getVectorForPosition(newPosition));
		transGroup.setTransform(transform);

		if (showBody)
			updateBody();
	}

	protected BranchGroup createBranchGroup()
	{
		BranchGroup objectRoot = new BranchGroup();
		objectRoot.setCapability(BranchGroup.ALLOW_DETACH);
		objectRoot.setCapability(Group.ALLOW_CHILDREN_EXTEND);
		objectRoot.setCapability(Group.ALLOW_CHILDREN_WRITE);

		BranchGroup positionsBranchGroup = new BranchGroup();
		positionsBranchGroup.setCapability(BranchGroup.ALLOW_DETACH);
		positionsBranchGroup.setCapability(Group.ALLOW_CHILDREN_EXTEND);
		positionsBranchGroup.setCapability(Group.ALLOW_CHILDREN_WRITE);

		bodyBranchGroup = new BranchGroup();
		bodyBranchGroup.setCapability(BranchGroup.ALLOW_DETACH);
		bodyBranchGroup.setCapability(Group.ALLOW_CHILDREN_EXTEND);
		bodyBranchGroup.setCapability(Group.ALLOW_CHILDREN_WRITE);
		positionsBranchGroup.addChild(bodyBranchGroup);

		tagsBranchGroup = new BranchGroup();
		tagsBranchGroup.setCapability(BranchGroup.ALLOW_DETACH);
		tagsBranchGroup.setCapability(Group.ALLOW_CHILDREN_WRITE);
		tagsBranchGroup.setCapability(Group.ALLOW_CHILDREN_EXTEND);
		positionsBranchGroup.addChild(tagsBranchGroup);

		objectRoot.addChild(positionsBranchGroup);

		// add room
		LineStripArray room = getRoom();
		objectRoot.addChild(new Shape3D(room));

		// add lights
		double RADIUS = 100.0;
		objectRoot.addChild(getDirectinalLight(0.0, 0.0, 20.0, 0.0f, 0.0f, -1.0f, RADIUS)); // top
		objectRoot.addChild(getDirectinalLight(0.0, 0.0, -20.0, 0.0f, 0.0f, 1.0f, RADIUS)); // bottom
		objectRoot.addChild(getDirectinalLight(-20.0, 0.0, 0.0, 1.0f, 0.0f, 0.0f, RADIUS)); // left
		objectRoot.addChild(getDirectinalLight(20.0, 0.0, 0.0, -1.0f, 0.0f, 0.0f, RADIUS)); // right
		objectRoot.addChild(getDirectinalLight(0.0, -20.0, 0.0, 0.0f, 1.0f, 0.0f, RADIUS)); // front
		objectRoot.addChild(getDirectinalLight(0.0, 20.0, 0.0, 0.0f, -1.0f, 0.0f, RADIUS)); // back

		objectRoot.addChild(transformGroup);

		// compile
		objectRoot.compile();
		return objectRoot;
	}

	private Position calculateReferencePosition()
	{
		Position frontPos = filteredTagPositions.get(TagAssignment.FRONT);
		Position backPos = filteredTagPositions.get(TagAssignment.BACK);

		// calculate mean of positions
		double x = frontPos.getX() + backPos.getX();
		x /= 2.0;
		double y = frontPos.getY() + backPos.getY();
		y /= 2.0;
		double z = frontPos.getZ() + backPos.getZ();
		z /= 2.0;

		// add z offset to z variable
		double zOffset = Application.getInstance().getZOffset();
		z += zOffset;

		return new Position("reference", x, y, z, Helper.now());
	}

	private Appearance getAppearance(Color color)
	{
		Appearance appearance = new Appearance();

		Color3f color3f = new Color3f(Color.YELLOW);
		if (color != null)
			color3f.set(color);

		Material material = new Material();
		material.setDiffuseColor(color3f);
		appearance.setMaterial(material);

		ColoringAttributes coloringAttributes = getColoringAttributes(color3f);
		appearance.setColoringAttributes(coloringAttributes);

		return appearance;
	}

	private ColoringAttributes getColoringAttributes(Color3f color3f)
	{
		return new ColoringAttributes(color3f, ColoringAttributes.FASTEST);
	}

	private DirectionalLight getDirectinalLight(double xOff, double yOff, double zOff, float xDir, float yDir,
			float zDir, double radius)
	{
		Color3f lightColor = new Color3f(Color.WHITE);
		Point3d point = new Point3d(centerOfRoom);
		point.add(new Point3d(xOff, yOff, zOff));
		BoundingSphere bounding = new BoundingSphere(point, radius);
		Vector3f lightdirection = new Vector3f(xDir, yDir, zDir);

		DirectionalLight directionalLight = new DirectionalLight(lightColor, lightdirection);
		directionalLight.setInfluencingBounds(bounding);
		return directionalLight;
	}

	private List<Point3d> getFloor(double height)
	{
		List<Point3d> list = new ArrayList<Point3d>(5);
		list.add(new Point3d(offset.x, offset.y, offset.z + height));
		list.add(new Point3d(offset.x + width, offset.y, offset.z + height));
		list.add(new Point3d(offset.x + width, offset.y + length, offset.z + height));
		list.add(new Point3d(offset.x, offset.y + length, offset.z + height));
		list.add(new Point3d(offset.x, offset.y, offset.z + height));
		return list;
	}

	private List<Point3d> getNormals()
	{
		List<Point3d> list = new ArrayList<Point3d>(8);
		list.add(offset);
		list.add(new Point3d(offset.x, offset.y, offset.z + height));

		list.add(new Point3d(offset.x + width, offset.y, offset.z));
		list.add(new Point3d(offset.x + width, offset.y, offset.z + height));

		list.add(new Point3d(offset.x + width, offset.y + length, offset.z));
		list.add(new Point3d(offset.x + width, offset.y + length, offset.z + height));

		list.add(new Point3d(offset.x, offset.y + length, offset.z));
		list.add(new Point3d(offset.x, offset.y + length, offset.z + height));
		return list;
	}

	private List<Point3d> getPointListForPositions(Position... positions)
	{
		List<Point3d> list = new ArrayList<Point3d>(positions.length);
		for (int i = 0; i < positions.length; i++)
		{
			Position pos = positions[i];
			Point3d tmp = new Point3d();
			tmp.x = pos.getX();
			tmp.y = pos.getY();
			tmp.z = pos.getZ();
			list.add(tmp);
		}
		return list;
	}

	private Point3d[] getPointArrayForPositions(Position... positions)
	{
		List<Point3d> list = getPointListForPositions(positions);
		return list.toArray(new Point3d[positions.length]);
	}

	private LineStripArray getRoom()
	{
		Point3d[] floor = new Point3d[] { new Point3d(0.0, 0.0, 0.0), new Point3d(width, 0.0, 0.0),
				new Point3d(width, length, 0.0), new Point3d(0.0, length, 0.0), new Point3d(0.0, 0.0, 0.0),
				new Point3d(0.0, 0.0, height), new Point3d(width, 0.0, height), new Point3d(width, length, height),
				new Point3d(0.0, length, height), new Point3d(0.0, 0.0, height) };
		int vertexCount = floor.length;

		LineStripArray room = new LineStripArray(vertexCount, GeometryArray.COORDINATES, new int[] { floor.length });
		room.setCoordinates(0, floor);

		room = new LineStripArray(18, GeometryArray.COORDINATES, new int[] { 5, 5, 2, 2, 2, 2 });
		List<Point3d> points = new LinkedList<Point3d>();
		points.addAll(getFloor(0.0));
		points.addAll(getFloor(height));
		points.addAll(getNormals());
		room.setCoordinates(0, points.toArray(new Point3d[points.size()]));

		return room;
	}

	private Primitive getShapeForFilteredTag(TagAssignment tagAssignment)
	{
		Color color = Application.getInstance().getColorForTagPosition(tagAssignment);
		Appearance appearance = getAppearance(color.brighter());
		return new Box(0.05f, 0.05f, 0.05f, Primitive.GENERATE_NORMALS, appearance);
	}

	private Primitive getShapeForTag(TagAssignment tagAssignment)
	{
		Color color = Application.getInstance().getColorForTagPosition(tagAssignment);
		Appearance appearance = getAppearance(color.darker());
		return new Sphere(0.05f, Primitive.GENERATE_NORMALS, 9, appearance);
	}

	private Vector3d getVectorForPosition(Position newPosition)
	{
		return new Vector3d(newPosition.getX(), newPosition.getY(), newPosition.getZ());
	}
}
